
#include <nds.h>
#include <stdio.h>
#include <math.h>
#include <maxmod9.h>

#include "player.h"
#include "laser.h"  
#include "blandbackground.h"
#include "vec.h"
#include "entity.h"
#include "splitter.h"
#include "reflecter.h"
#include "redirect.h"
#include "boring.h"
#include "adequate_corgi.h"

#include "soundbank.h"
#include "soundbank_bin.h"

#define LASERS 64

void initBackgrounds();
void displayBackgrounds();      
void initVideo();

static const int DMA_CHANNEL = 3;

void initVideo() { 
  /*
   *  Map VRAM to display a background on the main and sub screens.
   * 
   *  The vramSetMainBanks function takes four arguments, one for each of the
   *  major VRAM banks. We can use it as shorthand for assigning values to
   *  each of the VRAM bank's control registers.
   *
   *  We map banks A and B to main screen  background memory. This gives us
   *  256KB, which is a healthy amount for 16-bit graphics.
   *
   *  We map bank C to sub screen background memory.
   *
   *  We map bank D to LCD. This setting is generally used for when we aren't
   *  using a particular bank.
   */
  vramSetMainBanks(VRAM_A_MAIN_BG_0x06000000,
                   VRAM_B_MAIN_BG_0x06020000,
                   VRAM_C_SUB_BG_0x06200000,
                   VRAM_D_SUB_SPRITE);
  
  vramSetBankE(VRAM_E_MAIN_SPRITE);
  
  videoSetMode(MODE_5_2D |
               DISPLAY_BG3_ACTIVE |
               DISPLAY_SPR_ACTIVE |
               DISPLAY_SPR_1D);
  videoSetModeSub(MODE_5_2D |
                  DISPLAY_BG3_ACTIVE | 
                  DISPLAY_SPR_ACTIVE |
                  DISPLAY_SPR_1D);
}



void initBackgrounds(){ 
  /*  Set up affine background 3 on main as a 16-bit color background. */
  REG_BG3CNT = BG_BMP16_256x256 |
    BG_BMP_BASE(0) | // The starting place in memory
    BG_PRIORITY(3); // A low priority
  
  REG_BG3PA = 3 << 5;
  REG_BG3PB = 0;
  REG_BG3PC = 0;
  REG_BG3PD = 3 << 5;
  
  REG_BG3X = 100 << 8;
  REG_BG3Y = 80 << 8;
  
  /*  Set up affine background 3 on the sub screen as a 16-bit color
   *  background.
   */
  REG_BG3CNT_SUB = BG_BMP16_256x256 |
    BG_BMP_BASE(0) | // The starting place in memory
    BG_PRIORITY(3); // A low priority
  
  /*  Set the affine transformation matrix for the sub screen background 3
   *  to be the identity matrix.
   */
  REG_BG3PA_SUB = 1 << 8;
  REG_BG3PB_SUB = 0;
  REG_BG3PC_SUB = 0;
  REG_BG3PD_SUB = 1 << 8;
  
  /*
   *  Place main screen background 3 at the origin (upper left of the screen)
   */
  REG_BG3X_SUB = 0;
  REG_BG3Y_SUB = 0;
}


void displayBackgrounds(){ 
  dmaCopyHalfWords(DMA_CHANNEL,
                   blandbackgroundBitmap,
                   (uint16 *)BG_BMP_RAM(0),
                   blandbackgroundBitmapLen);
  
  dmaCopyHalfWords(DMA_CHANNEL,
                   blandbackgroundBitmap,
                   (uint16 *)BG_BMP_RAM_SUB(0),
                   blandbackgroundBitmapLen);
}

int fireLaser(int x, int y, int angle,
               int laserPower, int laserPalette,
               const u16* laserGfx, const u16* laserSub,
               int* lasersUsedMain, int* lasersUsedSub,
               int* matrixMain, int* matrixSub,
               Entity** colliders, int numEntities) {
  if ((y > 0 ? *matrixMain : *matrixSub) >= 32) {
    return 0;
  }

  int confirmed_kills = 0;

  float max_laser_dist = 64.0f;
  float angle_cos = fixedToFloat(cosLerp(angle), 12);
  float angle_sin = fixedToFloat(sinLerp(angle), 12);
  
  // check for boundaries
  bool hit_wall = false;
  float x_wall_dist = (255 - x) / angle_cos;
  if (x_wall_dist < 0) {
    x_wall_dist = - x / angle_cos;
  }
  
  float y_wall_dist;
  bool hit_center = true;
  if (y > 0) {
    y_wall_dist = -y / angle_sin;
    if (y_wall_dist < 0) {
      hit_center = false;
      y_wall_dist = (SCREEN_HEIGHT + 32 - y) / angle_sin;
    }
  } else {
        y_wall_dist = -y / angle_sin;
        hit_center = true;
        if (y_wall_dist < 0) {
                hit_center = false;
                y_wall_dist = -(SCREEN_HEIGHT + y) / angle_sin;
        }
  }
  
  float dist = y_wall_dist;
  if (x_wall_dist < y_wall_dist) {
    dist = x_wall_dist;
    hit_center = false;
    hit_wall = (y < 0);
  }

  if (dist < 1) {
    return 0;
  }

  Entity* collided = colliders[1];
  bool did_collide = false;
  for (int i = 0; i < numEntities; ++i) {
    float t = colliders[i]->collisionTime(x, y, angle_cos, angle_sin);
    if (((i == 0 && t > 50) || t > 0 ) && t < dist) {
      dist = t;
      hit_center = false;
      hit_wall = false;
      did_collide = true;
      collided = colliders[i];
    }
  }
  
  int sprites_needed = ceil(dist / max_laser_dist);
  if (sprites_needed + (y > 0 ? *lasersUsedMain : *lasersUsedSub) > LASERS) {
    return 0;
  }
  float dist_per_sprite = dist / sprites_needed;
  float sprite_scale = dist_per_sprite / 32;
  
  int offset_x = (int)(dist_per_sprite*angle_cos);
  int offset_y = (int)(dist_per_sprite*angle_sin);
  
  int temp_x = x + offset_x / 2;
  int temp_y = y + offset_y / 2;
  if (y > 0) {
    oamRotateScale(&oamMain, *matrixMain, -angle,
                   floatToFixed(1 / sprite_scale, 8), 
                   floatToFixed(1/(pow(laserPower, 4) / pow(100, 4) + 0.25),
                                8));
	*matrixMain += 1;
  } else {
    oamRotateScale(&oamSub, *matrixSub, -angle,
                   floatToFixed(1 / sprite_scale, 8), 
                   floatToFixed(1/(pow(laserPower, 4) / pow(100, 4) + 0.25),
                                8));
	*matrixSub += 1;
  }
  for (int i = 0; i < sprites_needed; ++i) {
    if (temp_y > 0) {
      oamSet(&oamMain, 64 + i + *lasersUsedMain, temp_x - 32, temp_y - 64, 2,
             laserPalette, SpriteSize_32x32, SpriteColorFormat_16Color,
             laserGfx, *matrixMain - 1, true, false, false, false, false);
    } else {
      oamSet(&oamSub, 64 + i + *lasersUsedSub,
             temp_x - 32, SCREEN_HEIGHT + temp_y - 32, 2,
             laserPalette, SpriteSize_32x32, SpriteColorFormat_16Color,
             laserSub, *matrixSub - 1, true, false, false, false, false);
    }
    temp_x += offset_x;
    temp_y += offset_y;
  }
  if (y > 0) {
    *lasersUsedMain += sprites_needed;
  } else {
    *lasersUsedSub += sprites_needed;
  }
  
  if (hit_center) {
    int new_x = x + (int)(dist * angle_cos);
    confirmed_kills += fireLaser(new_x, (y > 0) ? -1 : 1, angle,
				 laserPower, laserPalette,
				 laserGfx, laserSub,
				 lasersUsedMain, lasersUsedSub,
				 matrixMain, matrixSub,
				 colliders, numEntities);
  }
  if (hit_wall) {
    int new_x = x + (int)(dist * angle_cos);
    if (new_x > SCREEN_WIDTH - 5) {
      new_x = SCREEN_WIDTH - 5;
    }
    int new_y = y + (int)(dist * angle_sin);
    Vec angle2(-angle_cos, angle_sin);
    confirmed_kills += fireLaser(new_x, new_y, angle2.angle(),
				 laserPower, laserPalette,
				 laserGfx, laserSub,
				 lasersUsedMain, lasersUsedSub,
				 matrixMain, matrixSub,
				 colliders, numEntities);
  }
  if (did_collide) {
    
    int collision_x = x + (int)(dist * angle_cos);
    int collision_y = y + (int)(dist * angle_sin);
    int angles[5];
    int num_angles = collided->reflectAngle(collision_x, collision_y,
					    angle_cos, angle_sin,
					    angles);
    for (int i = 0; i < num_angles; ++i) {
      confirmed_kills += fireLaser(collided->x + 16,collided->y + 16, angles[i],
				   laserPower, laserPalette,
				   laserGfx, laserSub,
				   lasersUsedMain, lasersUsedSub,
				   matrixMain, matrixSub,
				   colliders, numEntities);
    }
    collided->getHit(laserPower);
    if (collided->health <= 0) {
      confirmed_kills++;
    }
  }
  return confirmed_kills;
}

void hideLaser() {
  for (int i = 0; i < LASERS; ++i) {
    oamSet(&oamMain, 64 + i, 0, 0, 2, 0,
           SpriteSize_32x32, SpriteColorFormat_16Color,
           (void*)0, 0, true, true, false, false, false);
    oamSet(&oamSub, 64 + i, 0, 0, 2, 0,
           SpriteSize_32x32, SpriteColorFormat_16Color,
           (void*)0, 0, true, true, false, false, false);
  }
}


//---------------------------------------------------------------------------------
int main(void) {
  //---------------------------------------------------------------------------------
  powerOn(POWER_ALL_2D);
  
  lcdMainOnBottom(); // Place the main screen on the bottom physical screen
  initVideo();
  initBackgrounds();
  
  //consoleDemoInit();

  mmInitDefaultMem((mm_addr)soundbank_bin);
  mmLoadEffect( SFX_LASER );
  
  mm_sound_effect laser = {
    { SFX_LASER },
    1 << 10,
    0,
    255,
    128 };
  
  mmLoadEffect( SFX_NOISE );
  
  mm_sound_effect noise = {
    { SFX_NOISE },
    1 << 10,
    0,
    255,
    128 };
  
  mmLoadEffect( SFX_BEAT );
  
  mm_sound_effect beat = {
    { SFX_BEAT },
    1 << 10,
    0,
    255,
    128 };
  
  mm_sfxhand noise_handle = 0;
  mm_sfxhand beat_handle = 0;


  oamInit(&oamMain, SpriteMapping_1D_32, false);
  oamInit(&oamSub, SpriteMapping_1D_32, false);
  
  u16* playerMain;
  u16* playerSub;
  int palette = Entity::initGfx(playerPal, playerPalLen,
                                playerTiles, playerTilesLen,
                                &playerMain, &playerSub);
  
  BoringEntity player(SCREEN_WIDTH / 2, SCREEN_HEIGHT / 2 + 32,
                          palette, playerMain, playerSub);
  player.health = 30;

  u16* splitterMain;
  u16* splitterSub;
  int splitterPalette = Entity::initGfx(splitterPal, splitterPalLen,
					splitterTiles, splitterTilesLen,
					&splitterMain, &splitterSub);

  u16* reflecterMain;
  u16* reflecterSub;
  int reflecterPalette = Entity::initGfx(reflecterPal, reflecterPalLen,
					 reflecterTiles, reflecterTilesLen,
					 &reflecterMain, &reflecterSub);

  u16* redirectMain;
  u16* redirectSub;
  int redirectPalette = Entity::initGfx(redirectPal, redirectPalLen,
					  redirectTiles, redirectTilesLen,
					  &redirectMain, &redirectSub);

  u16* boringMain;
  u16* boringSub;
  int boringPalette = Entity::initGfx(boringPal, boringPalLen,
				      boringTiles, boringTilesLen,
				      &boringMain, &boringSub);
  
  int numEnemies = LASERS;
  int numEnemiesLeft = numEnemies - 1;
  int numReflecters = numEnemiesLeft / 10;
  numEnemiesLeft -= numReflecters;
  int numSplitters = numEnemiesLeft / 3;
  numEnemiesLeft -= numSplitters;
  int numBoring = 2 * numEnemiesLeft / 3;
  numEnemiesLeft -= numBoring;
  int numRedirecters = numEnemiesLeft;
  
  Entity* enemies[numEnemies];
  enemies[0] = &player;  // That's right, you're Public Enemy #1... err... #0
  int enemy_num = 1;
  for (; enemy_num < numReflecters + 1; ++enemy_num) {
    enemies[enemy_num] = new ReflectiveEntity(0, 0, reflecterPalette,
					      reflecterMain, reflecterSub);
    enemies[enemy_num]->health = 0;
  }
  for (; enemy_num < numReflecters + 1 + numSplitters; ++enemy_num) {
    enemies[enemy_num] = new SplitterEntity(0, 0, splitterPalette,
					    splitterMain, splitterSub);
    enemies[enemy_num]->health = 0;
  }
  for (; enemy_num < numReflecters + 1 + numSplitters + numBoring;++enemy_num) {
    enemies[enemy_num] = new BoringEntity(0, 0, boringPalette,
					  boringMain, boringSub);
    enemies[enemy_num]->health = 0;
  }
  for (; enemy_num < numEnemies; ++enemy_num) {
    enemies[enemy_num] = new RedirectEntity(0, 0, redirectPalette,
					    redirectMain, redirectSub);
    enemies[enemy_num]->health = 0;
  }
  
  u16* laserMain;
  u16* laserSub;
  int laserPalette = Entity::initGfx(laserPal, laserPalLen,
                                     laserTiles, laserTilesLen,
                                     &laserMain, &laserSub);
  
  displayBackgrounds();
  
  float x = SCREEN_WIDTH / 2 - 16;
  float y = SCREEN_HEIGHT / 2 + 32;
  int laserPower = 100;

  Vec playerVel(0, 0);

  float friction = 0.8;
  int lasers_fired = 0;
  int laser_sprites_used = 0;
  int kills = 0;
  
  bool wasFiring = false;

  int enemy_quota = 5 + (kills / 10);

  int enemies_let_through = 0;
  bool lasered = true;
  bool collided = false;

  while(player.health > 0) {
    //beat_handle = mmEffectEx(&beat);

    // Handle keypresses
    float vx = 0;
    float vy = 0;
    scanKeys();
    if (keysHeld() & KEY_A) {
    }
    if (keysHeld() & KEY_UP) {
      vy -= 2;
    }
    if (keysHeld() & KEY_DOWN) {
      vy += 2;
    }
    if (keysHeld() & KEY_RIGHT) {
      vx += 2;
    }
    if (keysHeld() & KEY_LEFT) {
      vx -= 2;
    }
    if (keysHeld() & KEY_X) {
    }
    if (keysHeld() & KEY_Y) {
    }
    if (keysHeld() & KEY_L) {
    }
    if (keysHeld() & KEY_R) {
    }
    
    playerVel = playerVel.scale(friction);
    if (vx != 0 || vy != 0) {
      playerVel = playerVel.scale(friction/2).plus(Vec(vx, vy).unit().scale(2));
    }

    x += playerVel.x;
    y += playerVel.y;

    if (x < 5) {
      x = 5;
    }
    if (x > SCREEN_WIDTH - 37) {
      x = SCREEN_WIDTH - 37;
    }
    if (y < -SCREEN_HEIGHT + 5) {
      y = -SCREEN_HEIGHT + 5;
    }
    if (y > SCREEN_HEIGHT - 5) {
      y = SCREEN_HEIGHT - 5;
    }

    player.x = x;
    player.y = y;

    player.move((int)x, (int)y);
    hideLaser();

    int liveEnemies = 0;
    for (int i = 1; i < numEnemies; ++i) {
      // Get rid of the enemies that are off the map
      if (enemies[i]->health > 0 && enemies[i]->y > SCREEN_HEIGHT + 32) {
	enemies_let_through++;
      }
      enemies[i]->killThoseWhoWanderAstray();
      if (enemies[i]->health > 0) {
	liveEnemies++;
	enemies[i]->step();
	float dx = x - enemies[i]->x;
	float dy = y - enemies[i]->y;
	float sq_mag = pow(dx, 2) + pow(dy, 2);
	if (sq_mag < 100) {
	  player.health = 0;
	  collided = true;
	  lasered = false;
	}
      }
      enemies[i]->move((int) enemies[i]->x, (int) enemies[i]->y);
    }

    if (liveEnemies < enemy_quota && rand() % 500 < kills + 10) {
      Entity* lucky_winner = enemies[rand() % (numEnemies - 1) + 1];
      if (lucky_winner->health <= 0) {
	lucky_winner->takeThisGiftSoThatYouMayLiiiive(kills);
      }
    }
    
    if (enemies_let_through > 15) {
      player.health = 0;
      lasered = false;
    }

    touchPosition touch;
    touchRead(&touch);
    if (touch.rawx || touch.rawy) {
      if (!wasFiring) {
	lasers_fired++;
	mmEffectEx(&laser);
      }
      wasFiring = true;
      int pcx = x + 16;
      int pcy = y + 16;
      int dx = touch.px - pcx;
      int dy = (touch.py + 32) - pcy;
      
      int angle = (int)degreesToAngle(atan2(dy, dx)*180.0/3.14159);
      
      int mainUsed = 0;
      int subUsed = 0;
      int matrixMain = 0;
      int matrixSub = 0;
      
      kills += fireLaser(pcx, pcy, angle, laserPower,
			 laserPalette, laserMain, laserSub,
			 &mainUsed, &subUsed,
			 &matrixMain, &matrixSub,
			 enemies, numEnemies);

      laser_sprites_used += mainUsed + subUsed;
      
      laserPower-= 1;
      if (laserPower < 0) {
        laserPower = 0;
      }
    } else {
      laserPower += 4;
      if (laserPower > 100) {
        laserPower = 100;
	wasFiring = false;
      }
    }
    
    if (player.gotHit) {
      player.gotHit = false;
	  player.stepped = 2;
	  player.animate_stepright();
      mmEffectEx(&noise);
    }

    swiWaitForVBlank();
    oamUpdate(&oamMain);
    oamUpdate(&oamSub);
  }

  mmEffectCancel(beat_handle);

  int frameDelay = 120;
  while (frameDelay-- > 0) {
    swiWaitForVBlank();
  }

  consoleDemoInit();
  printf("\x1b[4;1HLooks like you");
  if (lasered) {
  printf("\x1b[5;1Hlasered yourself");
  } else if (collided) {
	printf("\x1b[5;1Hcollided with an enemy");
  } else {
	printf("\x1b[5;1Hlet too many enemies through");
  }

  frameDelay = 120;
  while(frameDelay-- > 0) { 
    swiWaitForVBlank();
  }

  printf("\x1b[7;2HKills: %d", kills - 1);
  
   frameDelay = 60;
  while(frameDelay-- > 0) { 
    swiWaitForVBlank();
  }
  
  if (lasered) {
    printf("\x1b[8;4H(plus yourself)");
    
    frameDelay = 60;
    while(frameDelay-- > 0) { 
      swiWaitForVBlank();
    }
  }
  
  printf("\x1b[9;2HLasers Fired: %d", lasers_fired);

  frameDelay = 60;
  while(frameDelay-- > 0) { 
    swiWaitForVBlank();
  }

  printf("\x1b[11;2HLaser Sprites Used:");
  printf("\x1b[12;4H%d", laser_sprites_used);

  frameDelay = 120;
  while(frameDelay-- > 0) { 
    swiWaitForVBlank();
  }
  
  printf("\x1b[15;8HYOUR GRADE:");
  
  frameDelay = 120;
  while(frameDelay-- > 0) { 
    swiWaitForVBlank();
  }
  
  for (int i = 0; i < numEnemies; ++i) {
	enemies[i]->move(0, -32);
  }
	hideLaser();
	oamUpdate(&oamMain);
  swiWaitForVBlank();
  
  REG_BG3PA = 1 << 8;
  REG_BG3PB = 0;
  REG_BG3PC = 0;
  REG_BG3PD = 1 << 8;
  
  REG_BG3X = 0;
  REG_BG3Y = 0;
  
   dmaCopyHalfWords(DMA_CHANNEL,
                   adequate_corgiBitmap,
                   (uint16 *)BG_BMP_RAM(0),
                   adequate_corgiBitmapLen);
				   
				     frameDelay = 1200;
  while(frameDelay-- > 0) { 
    swiWaitForVBlank();
  }
  
  return 0;
}

